/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package org.femto.math.linalg;

/**
 * Simple double 3-d vector class
 *
 * @author root
 */
public class Vec3 {

    public double[] v = new double[3];

    public Vec3() {
        zero();
    }

    public Vec3(double ix, double iy, double iz) {
        v[0] = ix; v[1] = iy; v[2] = iz;
    }

    public Vec3(Vec3 iv) {
        v[0] = iv.v[0]; v[1] = iv.v[1]; v[2] = iv.v[2];
    }

    public void zero() {
        v[0] = 0; v[1] = 0; v[2] = 0;
    }

    public Vec3 copy() {
        return new Vec3(this);
    }

    public void set(Vec3 u) {
        v[0] = u.v[0]; v[1] = u.v[1]; v[2] = u.v[2];
    }

    public void set(double ix, double iy, double iz) {
        v[0] = ix; v[1] = iy; v[2] = iz;
    }

    public Vec3 add(Vec3 u) {
        return add(this, u);
    }
    
    public static Vec3 add(Vec3 u, Vec3 v) {
        return new Vec3(u.v[0]+v.v[0], u.v[1]+v.v[1], u.v[2]+v.v[2]);
    }
    
    public void addInplace(Vec3 u) {
        v[0] += u.v[0]; v[1] += u.v[1]; v[2] += u.v[2];
    }

    public Vec3 sub(Vec3 u) {
        return sub(this, u);
    }

    public static Vec3 sub(Vec3 u, Vec3 v) {
        return new Vec3(u.v[0]-v.v[0], u.v[1]-v.v[1], u.v[2]-v.v[2]);
    }

    public Vec3 negative() {
        return new Vec3(-v[0], -v[1], -v[2]);
    }

    public void subInplace(Vec3 u) {
        v[0] -= u.v[0]; v[1] -= u.v[1]; v[2] -= u.v[2];
    }

    public Vec3 scale(double a) {
        return scale(this, a);
    }

    public static Vec3 scale(Vec3 u, double a) {
        return new Vec3(u.v[0]*a, u.v[1]*a, u.v[2]*a);
    }

    public void scaleInplace(double a) {
        v[0] *= a; 
        v[1] *= a;
        v[2] *= a;
    }

    public static Vec3 axpy(double a, Vec3 x, Vec3 y) {
        return new Vec3(a*x.v[0]+y.v[0], a*x.v[1]+y.v[1], a*x.v[2]+y.v[2]);
    }

    public void maddInplace(Vec3 u, double a) {
        v[0] += u.v[0]*a;
        v[1] += u.v[1]*a;
        v[2] += u.v[2]*a;
    }

    public double dot(Vec3 u) {
        return dot(this, u);
    }

    public static double dot(Vec3 u, Vec3 v) {
        return u.v[0]*v.v[0]+u.v[1]*v.v[1]+u.v[2]*v.v[2];
    }

    public static Vec3 cross(Vec3 u, Vec3 v) {
        return new Vec3(u.v[1]*v.v[2]-u.v[2]*v.v[1], u.v[2]*v.v[0]-u.v[0]*v.v[2], u.v[0]*v.v[1]-u.v[1]*v.v[0]);
    }

    public Vec3 cross(Vec3 u) {
        return cross(this, u);
    }

    public static double norm2(Vec3 u) {
        return dot(u,u);
    }

    public double norm2() {
        return norm2(this);
    }

    public static double norm(Vec3 u) {
        return Math.sqrt(norm2(u));
    }

    public double norm() {
        return norm(this);
    }

    public static double dist2(Vec3 u, Vec3 v) {
        return norm2(sub(u,v));
    }

    public static double dist(Vec3 u, Vec3 v) {
        return Math.sqrt(dist2(u,v));
    }

    public void normalizeSelf() {
        double norm = norm();
        if(norm > 0.0f) {
            double invNorm = 1.0/norm;
            v[0] *= invNorm;
            v[1] *= invNorm;
            v[2] *= invNorm;
        }
    }

    public Vec3 normalized() {
        Vec3 n = new Vec3(this);
        n.normalizeSelf();
        return n;
    }

    public static String toString(Vec3 u) {
        StringBuffer sb = new StringBuffer(100);
        sb.append("").append(u.v[0]).append(",").append(u.v[1]).append(",").append(u.v[2]).append("");
        return sb.toString();
    }

    public String toString() {
        return toString(this);
    }

    public double[] toArray() {
        return v;
    }

    /** parse a MATLAB-style [ x y z ] vector */
    public static Vec3 parseVec3(String s) {
        java.util.Scanner scanner = new java.util.Scanner(s);
        scanner.useDelimiter("\\s*[\\x5B\\x5D]\\s*|\\s+");

        double x = scanner.nextDouble();
        double y = scanner.nextDouble();
        double z = scanner.nextDouble();

        scanner.close();
        return new Vec3(x, y, z);
    }
}
